#include <stdlib.h>
#include "BezierInterpolationMap.h"

BezierInterpolationMap* newBezierInterpolationMap() {
	BezierInterpolationMap* map;
	map = (BezierInterpolationMap*) malloc(sizeof(BezierInterpolationMap));
	map->init();
	return map;
}

void BezierInterpolationMap::init() {
	minInputValue = 0;
	maxInputValue = 1023;
	minOutputValue = 0;
	maxOutputValue = 255;
}

void BezierInterpolationMap::setControlPoint1(int x, int y) {
	controlPoint1.x = x;
	controlPoint1.y = y;
}

void BezierInterpolationMap::setControlPoint2(int x, int y) {
	controlPoint2.x = x;
	controlPoint2.y = y;
}

int BezierInterpolationMap::mapInputValue(int inputValue) {
	if (inputValue < minInputValue) {
		inputValue = minInputValue;
	} else if (inputValue > maxInputValue) {
		inputValue = maxInputValue;
	}

	float u = getStep(inputValue);
	Coordinate forCoordinate;
	calculateCoordinate(forCoordinate, u);
	return forCoordinate.y;
}

float BezierInterpolationMap::getStep(int inputValue) {
	float max = (float) maxInputValue;
	float in = (float) inputValue;
	float u = in / max;
	return u;
}

void BezierInterpolationMap::lerp(Coordinate &forCoordinate, Coordinate &a, Coordinate &b, float u) {
	forCoordinate.x = a.x + (b.x - a.x) * u;
	forCoordinate.y = a.y + (b.y - a.y) * u;
}

void BezierInterpolationMap::calculateCoordinate(Coordinate &forCoordinate, float u) {
	// Todo: Dynamic number of control points, loop here over coordinates
	Coordinate a, d, ab, bc, cd, abbc, bccd;
	d.x = maxInputValue;
	d.y = maxOutputValue;
	lerp(ab, a, controlPoint1, u);
	lerp(bc, controlPoint1, controlPoint2, u);
	lerp(cd, controlPoint2, d, u);
	lerp(abbc, ab, bc, u);
	lerp(bccd, bc, cd, u);
	lerp(forCoordinate, abbc, bccd, u);
}
